Add methods for caching and retrieving trajectories #541

madan96 · 2019-08-16T03:20:12Z

This PR addresses #534 and adds methods to save and load a spline trajectory using YAML file. The methods are implemented as utility functions under src/trajectory.

This is an example of how a saved trajectory file looks like:

start_time: 0
dofs: 7
traj_order: 3
num_segments: 2
seg_0:
  coefficients: [4.07281e-20,0,-0.00101443,-2.925e-14,3.69597e-18,3.90809e-17,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
  duration: 0.1875
  start_state: [3.14159,7.38502e-17,0,0,0,0,0]
seg_1:
  coefficients: [3.35408e-20,-0.000380411,-0.00101443,4.69822e-14,1.47196e-17,-0.375,-1,2.39233e-14,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
  duration: 0.1875
  start_state: [3.14156,-0.0351562,0,0,0,0,0]

I have added a unit test to check trajectory loaded from a saved YAML file has the same member variables as that of the original trajectory. Additionally, as a sanity check, also tested the methods with herb3_demos/simple_trajectories by loading saved trajectories for execution and visualization it in RViz.

Before creating a pull request

Document new methods and classes
Format code with make format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

* Status: Working; Tested with simple_trajectories

brianhou

Thanks for the great start! I have a few thoughts and questions:

I think the trajectory loading should be in aikido::io rather than aikido::trajectory. Would there be some weird dependencies, or does that make sense?
Rather than manually writing out the information in YAML format, can we take advantage of the YAML emitter for these sequences? In particular, I think aikido/io/detail/yaml_extension.hpp has some stuff for Eigen vectors that might be nice to use, so we don't have to specify an Eigen::IOFormat here.

Thoughts about the format:

I think dofs should refer to a list of DOF names (and doesn't need to just be a sequence). This might allow us to animate other objects in the future by specifying those joints.
I think some of this information might be redundant, e.g. num_segments might be obvious from the number of segments.
I'm not sure whether we need specific names for each segment; maybe we can just put them in a sequence and handle that implicitly?
Maybe it makes sense to organize the YAML information into two maps, similar to OpenRave.

So all together, maybe it could look something like this:

configuration:
  start_time: 0
  dofs: ["/right/j1", "/right/j2", ...]
  type: spline
  spline:
    order: 3
data:
  - coefficients: [1, 2, ...]
    duration: 0.1875
    start_state: [3, 4, ...]
  - coefficients: [5, 6, ...]
    duration: 0.1875
    start_state: [7, 8, ...]

brianhou · 2019-08-17T00:43:11Z

src/trajectory/util.cpp

+  {
+    auto segment_id = "seg_" + std::to_string(i);
+    Eigen::MatrixXd coeffVec
+        = trajFile[segment_id]["coefficients"].as<Eigen::MatrixXd>();


Why MatrixXd rather than VectorXd? It seems to just be a vector in the yaml file?

No particular reason, while implementing I may have associated MatrixXd with all coefficient related variables I used. But yes, having this as VectorXd definitely makes more sense.

madan96 · 2019-08-17T01:42:17Z

I thought to have this implementation in aikido::trajectory since it is something very specific to trajectories and sounded more like a utility function provided along with trajectory. Also, I'm not too familiar with aikido::io but I was of the impression that it has classes/methods that are generic and used by multiple submodules in aikido.
I didn't know about YAML emitters but looks really helpful. I think I can definitely rewrite the YAML related part using this and helper function implemented in aikido/io/detail/yaml_extension.hpp

The format that you've suggested actually looks great and is more clearer. Would the entries under data block be indexed? This was one of the reasons why I used num_segments and segment names since I couldn't figure out a simple way to parse all the segments without knowing the exact number while loading.

brianhou · 2019-08-17T19:56:06Z

It seems that aikido::io mainly defines convenience functions for robot library or demo-level scripts, not other AIKIDO modules. The current dependencies seem to suggest that only perception and robot depend on io at the moment, neither of which are dependencies for the core planning modules.

So, I think I'm still inclined to put this with other deserialization code (for raw YAML, kinbody, URDF formats) in io. It means that io is the main component that depends on yaml-cpp. @gilwoolee any thoughts?

I think after yaml-cpp parses the sequence for you, you should be able to then query for the size of that sequence just like any other C++ container :)

brianhou · 2019-08-21T20:28:47Z

include/aikido/io/trajectory.hpp

+/// \param[in] skelSpace Metaskeleton of the object
+/// \param[in] savePath save path for the trajectory yaml file
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& skelSpace,


Is this parameter necessary? I think it should be read from the trajectory.

brianhou · 2019-08-21T20:30:05Z

include/aikido/io/trajectory.hpp

+namespace aikido {
+namespace io {
+
+/// Saves a timed trajectory


I don't think we need this full summary. I'd just rephrase as "Serializes a spline trajectory to YAML." before listing the parameters.

brianhou · 2019-08-21T20:30:34Z

include/aikido/io/trajectory.hpp

+    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& skelSpace,
+    const std::string& savePath);
+
+/// Load spline trajectory from yaml file


Similarly, I'd just say that this "Deserializes a spline trajectory from YAML.".

brianhou · 2019-08-21T20:34:19Z

src/io/trajectory.cpp

+namespace io {
+
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const MetaSkeletonStateSpacePtr& skelSpace, const std::string& savePath)


As above, I think you should be able to just pull the statespace from the trajectory. I would just try to cast it to a MetaSkeletonStateSpace to get the properties you need.

If you do the above, I suggest using MetaSkeletonStateSpace specific functions to make the log/exp conversions.

@aditya-vk Is there any particular reason for that? Because I think using MetaSkeletonStateSpace specific function would first require type-casting the state that we pass to these functions.

src/io/trajectory.cpp

brianhou · 2019-08-21T20:41:25Z

src/io/trajectory.cpp

+      = ::aikido::common::make_unique<Spline>(stateSpace, startTime);
+
+  const YAML::Node& segments = trajFile["data"];
+  for (std::size_t i = 0; i < segments.size(); i++) {


The open brace should be on the next line.

Can we iterate directly over segments rather than by index?

Yes I guess we can use an iterator (with for loop) over the YAML node. Would that be better?

brianhou · 2019-08-21T20:42:33Z

src/io/trajectory.cpp

+    Eigen::VectorXd position = segment["start_state"].as<Eigen::VectorXd>();
+
+    // Convert position Eigen vector to StateSpace::State*
+    aikido::statespace::StateSpace::State* startState


@aditya-vk is this correct usage? Do we need to free the state?

Definitely. Needs a freeState() at the end of the function.

brianhou · 2019-08-21T20:46:39Z

src/io/trajectory.cpp

+
+  statespace::ConstStateSpacePtr trajSpace = trajectory.getStateSpace();
+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "configuration" << YAML::BeginMap;


Put the YAML::BeginMap on a new line?

brianhou · 2019-08-21T20:47:03Z

src/io/trajectory.cpp

+  emitter << YAML::BeginMap;
+  emitter << YAML::Key << "order" << YAML::Value << trajectory.getNumDerivatives();
+  emitter << YAML::EndMap;
+  emitter << YAML::EndMap << YAML::Key << "data" << YAML::BeginSeq;


Put the YAML::Key << "data" << YAML::BeginSeq on two more lines? I'd also move this right before the for loop.

include/aikido/io/trajectory.hpp

aditya-vk

Looks good! Requested some changes.

aditya-vk · 2019-08-21T21:11:01Z

include/aikido/io/trajectory.hpp

+/// Given trajectory file and trajectory state space, this method parses
+/// the trajectory file and loads a timed trajectory for direct execution.
+/// \param[in] trajPath Spline trajectory
+/// \param[in] stateSpace Metaskeleton for the trajectory


If we expect the stateSpace to be associated with a MetaSkeleton, I would rename it to metaSkeletonStateSpace. Additionally, MetaSkeleton -> MetaSkeletonStateSpace.

aditya-vk · 2019-08-21T21:12:11Z

include/aikido/io/trajectory.hpp

+} // namespace io
+} // namespace aikido
+
+#endif // AIKIDO_IO_TRAJECTORY_HPP_


Add EOF line.

src/CMakeLists.txt

aditya-vk · 2019-08-21T21:16:33Z

src/io/trajectory.cpp

+namespace io {
+
+void saveTrajectory(const aikido::trajectory::Spline& trajectory,
+    const MetaSkeletonStateSpacePtr& skelSpace, const std::string& savePath)


If you do the above, I suggest using MetaSkeletonStateSpace specific functions to make the log/exp conversions.

aditya-vk · 2019-08-21T21:20:00Z

src/io/trajectory.cpp

+    Eigen::VectorXd position = segment["start_state"].as<Eigen::VectorXd>();
+
+    // Convert position Eigen vector to StateSpace::State*
+    aikido::statespace::StateSpace::State* startState


Definitely. Needs a freeState() at the end of the function.

src/io/trajectory.cpp

tests/io/test_trajectory_io.cpp

Use iterators over YAML segment sequences Compare dof names and spline type for deserialized trajectory

brianhou

Looks really close! Just a few minor nits to go.

include/aikido/io/trajectory.hpp

src/io/trajectory.cpp

tests/io/test_trajectory_io.cpp

madan96 · 2019-08-29T16:36:16Z

I think this is ready for the final review! I haven't run make format since it might make unwanted changes as I've a different version of clang-format on my (bionic) system.

brianhou

One last minor nit, then I think we're good to merge!

src/io/trajectory.cpp

Addressed by 36b6f86

madan96 added 5 commits August 14, 2019 20:52

Add saveTrajectory method to utils

cc8cb5f

Add loadSplineTrajectory method to trajectory utils

4385289

* Status: Working; Tested with simple_trajectories

Add string parameters and comments

6fd530b

Add tests for saving/loading trajectories

348f69c

Minor formatting changes

e3aea4a

madan96 requested a review from gilwoolee August 16, 2019 03:20

madan96 requested review from aditya-vk and brianhou as code owners August 16, 2019 03:20

madan96 changed the title ~~Enhancement/cache traj~~ Add methods for caching and retrieving trajectories Aug 16, 2019

madan96 changed the title ~~Add methods for caching and retrieving trajectories~~ [WIP] Add methods for caching and retrieving trajectories Aug 16, 2019

Save order of polynomial segment in yaml file

b6ed11b

madan96 changed the title ~~[WIP] Add methods for caching and retrieving trajectories~~ Add methods for caching and retrieving trajectories Aug 16, 2019

brianhou suggested changes Aug 17, 2019

View reviewed changes

madan96 added 2 commits August 20, 2019 13:21

Redesign yaml template and use emitters for writing yaml

a5bb844

Move caching/loading methods to aikido::io

d72016b

brianhou suggested changes Aug 21, 2019

View reviewed changes

aditya-vk previously requested changes Aug 21, 2019

View reviewed changes

brianhou and others added 2 commits August 21, 2019 17:57

Merge branch 'master' into enhancement/cache_traj

9da40c7

Remove dependency on metaskeleton

372ad2a

Use iterators over YAML segment sequences Compare dof names and spline type for deserialized trajectory

madan96 force-pushed the enhancement/cache_traj branch from d741c4c to 372ad2a Compare August 24, 2019 18:37

brianhou suggested changes Aug 27, 2019

View reviewed changes

Add suggested changes

36b6f86

madan96 force-pushed the enhancement/cache_traj branch from aefbc97 to 36b6f86 Compare August 27, 2019 22:37

brianhou suggested changes Aug 29, 2019

View reviewed changes

src/io/trajectory.cpp Outdated Show resolved Hide resolved

madan96 and others added 2 commits August 29, 2019 09:57

Throw runtime_error instead of assert

b45a92d

Format code.

fd09d0b

brianhou previously approved these changes Aug 29, 2019

View reviewed changes

madan96 added this to the Aikido 0.3.0 milestone Aug 30, 2019

Update CHANGELOG.md

a55b6e3

madan96 dismissed brianhou’s stale review via a55b6e3 August 30, 2019 17:15

brianhou approved these changes Aug 31, 2019

View reviewed changes

brianhou merged commit 6dafdc4 into master Aug 31, 2019

brianhou deleted the enhancement/cache_traj branch August 31, 2019 18:52

brianhou mentioned this pull request Sep 2, 2019

Caching a trajectory #534

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add methods for caching and retrieving trajectories #541

Add methods for caching and retrieving trajectories #541

madan96 commented Aug 16, 2019 •

edited

Loading

brianhou left a comment

brianhou Aug 17, 2019

madan96 Aug 17, 2019

madan96 commented Aug 17, 2019

brianhou commented Aug 17, 2019

brianhou Aug 21, 2019

brianhou Aug 21, 2019

brianhou Aug 21, 2019

brianhou Aug 21, 2019

aditya-vk Aug 21, 2019

madan96 Aug 23, 2019

brianhou Aug 21, 2019

brianhou Aug 21, 2019

madan96 Aug 23, 2019

brianhou Aug 25, 2019

brianhou Aug 21, 2019

aditya-vk Aug 21, 2019

brianhou Aug 21, 2019

brianhou Aug 21, 2019

aditya-vk left a comment

aditya-vk Aug 21, 2019

aditya-vk Aug 21, 2019

aditya-vk Aug 21, 2019

aditya-vk Aug 21, 2019

brianhou left a comment

madan96 commented Aug 29, 2019

brianhou left a comment

Add methods for caching and retrieving trajectories #541

Add methods for caching and retrieving trajectories #541

Conversation

madan96 commented Aug 16, 2019 • edited Loading

brianhou left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

madan96 commented Aug 17, 2019

brianhou commented Aug 17, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aditya-vk left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brianhou left a comment

Choose a reason for hiding this comment

madan96 commented Aug 29, 2019

brianhou left a comment

Choose a reason for hiding this comment

madan96 commented Aug 16, 2019 •

edited

Loading